% save('data/experiment-60min.mat', 'inst_count_vec', 'rt_vec');
% load('data/experiment-60min.mat');
classdef test_fake_lqr < handle 
    %UNTITLED Summary of this class goes here
    %   Detailed explanation goes here
    
    properties
        % model=[];
        inst_count_vec=[];
        rt_vec=[]; 
         m=[];
         m_inst_count_vec=[];
         m_rt_vec=[]; 
         samplingTime = 1;         
         lqr=[];
         x0=[];
    end
    
    methods
        %     load('data/experiment-60min.mat');
        % test_fake_lqr().build_model( inst_count_vec, rt_vec)
        function obj=test_fake_lqr()
            
        end
        
       function [m_x,d_x]=linearize(obj,x)
            m_x = mean(x);
            % d_x = x-m_x;
            d_x = x-ones(size(x,1),1)*m_x;
        end
        
        function d_x=linearize_around(obj,x,m_x)
            d_x = x-ones( size(x,1),1) * m_x ;
        end
        
        function x=delinearize_around(obj,d_x,m_x)
            x = m_x  + d_x;
        end
        
      
        function build_model(obj, inst_count_vec, rt_vec)
            obj.inst_count_vec=inst_count_vec;  
            obj.rt_vec=rt_vec;
            %----------
            n_st=1;% 1 state variable
            n_in= size(inst_count_vec,2);  %11 inputs
            n_out= size(rt_vec,2); % 1 output
            n_env=1; % environmental disturbances
            A=ones(n_st,n_st);
            B=ones(n_st,n_in);
            K=ones(n_st,n_env);
            C=ones(n_out,n_st);
            D=zeros(n_out,n_in);
            
            m_rt_vec=2;
            [  d_rt_vec]= obj.linearize_around(rt_vec , m_rt_vec);  %  2  seconds desired
            m_inst_count_vec=2;
            [ d_inst_count_vec] = obj.linearize_around(inst_count_vec,...
               ones(1, size(inst_count_vec,2))* m_inst_count_vec);
            
            u =  d_inst_count_vec;
            y = d_rt_vec;
            
            data=iddata(y, u , obj.samplingTime);
            m=idss(A,B,C,D,K,'Ts', obj.samplingTime,'DisturbanceModel','Estimate','InitialState','Estimate');
            m1=init(m);
            mm=pem(data,m1);

            obj.m=mm;
            obj.m_inst_count_vec=m_inst_count_vec; 
            obj.m_rt_vec = m_rt_vec; 
            
            %-------------------------------------------------
            % Design LQ-optimal gain K
%            K = lqry(sys,10,1)  % u = -Kx minimizes J(u)
            relativeCost=1000000;
            Q=relativeCost*eye(1); 
            R=eye(11,11); % diag(prices_amz().get_hourly_price); %eye(11);
            N=zeros(1,11); 
            sys = ss(mm,'m');
            sys_noise = ss(mm);
            % P = sys_noise(:,[1 1]);
            % [K,S,e] = lqr(mm.A,mm.B,Q,R,N)  doesnt consider noise
            % [K,S,e] = lqr(sys,Q,R,N) 
            % [K,S,e] = lqrd(sys.a,sys.b,Q,R,1);  -----
             [K,S,e] =  lqr(sys,Q,R,N)
            % Design Kalman state estimator Kest.
            % noise covariance data of the plant  
            Qk=eye(11);
            Rk=eye(1);
            Nk=zeros(11,1);
 %            [Kest,L,P] = kalman(sys,Qk,Rk,Nk,[1 1 1 1 1 1 1 1 1 1 1], [1])
            [Kest,L,P]  = kalman(sys_noise, 0, 100,0)
            % Kest = kalman(sys_noise,1,0.01)
           % [Kest,L,P] = kalman(sys_noise,1,0.01); 
            
            % Form LQG regulator = LQ gain + Kalman filter.
            F = lqgreg(Kest,K);
            obj.lqr=F;
            
%             clsys = feedback(sys_noise,F,+1);
            clsys = feedback(sys_noise,F,[1 2 3 4 5 6 7 8 9 10 11],[1],+1);
            
            %   distrb=randn(60,1)*0.1;
           %  distrb=sin((1:60)'*0.1)*4+randn(60,1)*0.1;
             distrb=([ones(1,20) zeros(1,40)])'*10+randn(60,1);
             u=[zeros(60,11) distrb]; 
              t= 0:1:60-1;
             %[y_c,t_c,x_c] =
             lsim(clsys,u,t); 
%             figure;   
            % [y_o,t_o,x_o] =
      %       lsim(sys_noise,u,t);
%             ltiview(clsys,'r-.',sys_noise,'m--');
% %              ltiview();
% %              ltiview(sys_noise);
%             u=UtilityLib(); 
%              figure;
%              plot( y_c,  u.color{1}); hold on; 
%              plot( y_c,  u.color{2}); 
%             rt_acc = obj.delinearize_around(rt_acc,m_rt_vec); 
%             plot(rt_acc); 

            %---------------------------------- way 1
%             noise=randn(60,2)*0.001; 
%             x_sys = mm.X0; % system state  for the first time
%             rt_acc=[];
%             u_opt_acc=[];
%             x_ctrl=0;
%             for i=1:60                
%                 rt_ = rt_vec(end,:);  % system output - kalman input for the first time                
%                 u_opt=F.c*x_ctrl + F.d * rt_;
%                 x_ctrl=F.a* x_ctrl + F.b * rt_;
%                 %------ system update ------
%                 rt_=sys_noise.c*x_sys + sys_noise.d*[u_opt ; noise(i,1)];
%                 x_sys =sys_noise.a*x_sys + sys_noise.b*[u_opt ; noise(i,1)];
%                 rt_acc=[rt_acc rt_];
%                 u_opt_acc=[u_opt_acc ; u_opt'];
%             end
%             rt_acc = obj.delinearize_around(rt_acc,m_rt_vec); 
%             plot(rt_acc); 
% 
%             %---------------------------------- way 2
%             %      inp=rand
%             noise=randn(60,2);
%             
%             x_kalm = mm.X0 % kalman state for the first time
%             x_sys = mm.X0; % system state  for the first time
%             x_lqr=0;
%             
%             rt_ = rt_vec(end,:);  % system output - kalman input for the first time
%             u_opt = inst_count_vec(end,:)';  % controller output-kalman input for the first time
%             
%             
%             rt_acc=[];            
%             u_opt_acc=[]; 
%             for i=1:60
%                 %------  kalman update ------
%                 tmp=Kest.c*x_kalm + Kest.d*[u_opt ; rt_]; 
%                 y1_e=tmp(1);
%                 x1_e = tmp(2); 
%                 x_kalm =Kest.a*x_kalm + Kest.b*[u_opt ; rt_]; 
%                                 
%                 %------ controller update ------
%                  u_opt =-1*K*x1_e;
%                  x_lqr=(Kest.a-L*Kest.c-(Kest.B-L*Kest.D)*K)*x1_e+L*rt_;
%                 % u_opt = [1 0 0 0 0 0 0 0 0 0 0]'; 
%                 
%                 %------ system update ------
%                 rt_=sys_noise.c*x_sys + sys_noise.d*[u_opt ; noise(i,1)];
%                 x_sys =sys_noise.a*x_sys + sys_noise.b*[u_opt ; noise(i,1)]; 
%                 rt_acc=[rt_acc rt_]; 
%                 u_opt_acc=[u_opt_acc ; u_opt'];
%             end
%         %    clsys = feedback(sys,F,+1);
            
        
        end        
        
    end
    
end

            